#include <routefinder.h>
#include <user.h>
#include <messages.h>
#include <model.h>


int Dijkstra(int a, int b, track_model *graph, int *path) {
  int dist[MAX_NODES];
  int prev[MAX_NODES];
  int seen[MAX_NODES];
  int seenCount = 0;
  
  int i, u;
  for (i = 0; i < MAX_NODES; i++) {
    dist[i] = INFINITY;
    prev[i] = UNDEFINED;
    seen[i] = FALSE;
  }  
  dist[a] = 0;
  
  while (seenCount < MAX_NODES) {
    int min = INFINITY;
    for (i = 0; i < MAX_NODES; i++) {
      if (!seen[i] && dist[i] < min) {
        min = dist[i];
        u = i;
      }
    }

    seen[u] = TRUE;
    seenCount++;
    
    if (u == b) break;
    
    track_node current = graph->nodes[u];
    int edges;
    
    if (current.type == NODE_SWITCH)
      edges = 3;
    else if (current.type == NODE_SENSOR)
      edges = 2;
    else if (current.type == NODE_STOP)
      edges = 1;
    
    for (i = 0; i < edges; i++) {        
      int v = current.edges[i].dest;
      if (seen[v]) continue;
      int alt = dist[u] + current.edges[i].distance;
      if (alt < dist[v]) {
        dist[v] = alt;
        prev[v] = u;
      }
    }
  }
  
  if (dist[b] == INFINITY) return FALSE;
  
  i = 0;
  int temp[MAX_NODES];
  while (prev[u] != UNDEFINED) {
    temp[i++] = u;
    u = prev[u];
  }
  
  int j;
  for (j = 0, i = i - 1; i >= 0; i--, j++)
    path[j] = temp[i];
  path[j] = UNDEFINED;
  
  return TRUE;
}

void RouteFinder() {
  
  int sender_tid;
  route_req msg;
  route_reply reply;

  track_model model;
  
  // notify parent
  char track;
  int parent_tid;
  Receive(&parent_tid, &track, 1);
  Reply(parent_tid, NULL, 0);
  
  TrackModelInit(&model, track);
  
  RegisterAs("route-finder");
  
  for (;;) {
    Receive(&sender_tid, (char*)&msg, sizeof(route_req));
    reply.result = Dijkstra(msg.a, msg.b, &model, msg.path);
    Reply(sender_tid, (char*)&msg, sizeof(route_reply));
  }
  Panic("RouteFinder", "Chuck Norris!");
}

int FindRoute(int a, int b, int *path) {
  int routefinder_tid = WhoIs("route-finder");
  route_req msg;
  route_reply reply;
  
  msg.a = a;
  msg.b = b;
  msg.path = path;
  Send(routefinder_tid, (char*)&msg, sizeof(route_req), (char*)&reply, sizeof(route_reply));
  
  return reply.result;
}
